//
// Created by An on 2025/7/8.
//
#include "tof.h"

#include "gpio.h"
#include "usart.h"
#include "cmsis_os.h"


#include <stdio.h>

// 16位寄存器写
HAL_StatusTypeDef VL53L1X_WriteReg16(uint16_t reg, uint8_t value) {
  return HAL_I2C_Mem_Write(&hi2c1, VL53L1X_I2C_ADDR, reg, I2C_MEMADD_SIZE_16BIT, &value, 1, HAL_MAX_DELAY);
}

// 16位寄存器读
HAL_StatusTypeDef VL53L1X_ReadReg16(uint16_t reg, uint8_t* value) {
  return HAL_I2C_Mem_Read(&hi2c1, VL53L1X_I2C_ADDR, reg, I2C_MEMADD_SIZE_16BIT, value, 1, HAL_MAX_DELAY);
}

// 裁剪初始化序列（关键部分）
void VL53L1X_SensorInit(void)
{
  // 软件复位
  VL53L1X_WriteReg16(0x0000, 0x00);  // Software standby
  HAL_Delay(1);

  // 关键初始化寄存器配置
  VL53L1X_WriteReg16(0x0002, 0x01);  // Start boot process
  HAL_Delay(100);  // 启动时间，等待芯片内部启动完成

  // 用户可设置成 boot 检查后再 init，这里假定芯片已经 boot 完成

  // 关键初始化配置（建议值） - 只写几个代表项
  VL53L1X_WriteReg16(0x002D, 0x01);  // VHV config
  VL53L1X_WriteReg16(0x0031, 0x02);  // PhaseCal config

  // 设置测距模式为连续模式准备
  VL53L1X_WriteReg16(0x000E, 0x01);  // mode config: continuous mode

  // 清除中断
  VL53L1X_WriteReg16(0x0086, 0x01);

  // 设置为 IDLE 状态，等待 StartRanging 调用
  VL53L1X_WriteReg16(0x000C, 0x00);
}

////变量需要左移1位，例如0x29<<1，0x30<<1
void VL53L1X_SetI2CAddress(uint16_t old_addr, uint8_t new_addr)
{
  uint8_t reg[3];

  reg[0] = 0x00;  // 寄存器地址高字节
  reg[1] = 0x01;  // 寄存器地址低字节
  reg[2] = new_addr >> 1; // 写入新的7位地址（未左移）

  HAL_I2C_Master_Transmit(&hi2c1, old_addr, reg, 3, HAL_MAX_DELAY);

  vTaskDelay(10); // 等待设置生效
}





void i2c_scan(){
  char tmp_msg[50];
  for (uint8_t address = 1; address < 127; address++) {
    if (HAL_I2C_IsDeviceReady(&hi2c1, address << 1, 1, 10) == HAL_OK) {
      sprintf(tmp_msg,"I2C device found at address: 0x%02X\n",address);
      HAL_UART_Transmit(&huart1,(uint8_t*)tmp_msg,sizeof tmp_msg,HAL_MAX_DELAY);
      //printf("I2C device found at address: 0x%02X\r\n", address);
    }
  }
}


void tof_init(){
  //等待上电100ms
  vTaskDelay(100);
  //关闭SHUT1对应的TOF模块
  HAL_GPIO_WritePin(SHUT1_GPIO_Port,SHUT1_Pin,GPIO_PIN_RESET);
  //将SHUT2对应的TOF模块修改地址为0x30
  VL53L1X_SetI2CAddress(0x29<<1,0x30<<1);
  //恢复SHUT1对应的TOF模块
  HAL_GPIO_WritePin(SHUT1_GPIO_Port,SHUT1_Pin,GPIO_PIN_SET);
  vTaskDelay(100);

}

void tof_test(){

}


